#include "ImageProgress.h"
#include "Observer.h"
#include "imgui3d_my_world.h"
#include "imgui_manager.h"
#include "imgui_my_adjust.h"
#include "implot_my_linesplot.h"
// imgui test only
// #include "imgui_test.h"
#include <string>
#include <thread>

#define debug_gui

int main()
{
    Observer observer;
    ImageProgress main_progress = ImageProgress(std::ref(observer));

    thread produce(&ImageProgress::ImageProducer, &main_progress);
    thread consume(&ImageProgress::ImageConsumer2, &main_progress);
    thread observer_th(&Observer::runJudgeLoop, &observer);

    // thread fitting(&ImageProgress::Fitting, &main_progress);
    // thread readport(&ImageProgress::ReadIMU2, &main_progress);
    // thread sendport(&ImageProgress::SendData2, &main_progress);
    //thread savevideo(&ImageProgress::VideoSave, &main_progress);

#ifdef debug_gui
    ImGuiManager manager;
    manager.Init(1400, 1200);
    // 所有控件每帧统一更新数据
    cv::Mat KF_Gain = cv::Mat();
    cv::Point3f observed = cv::Point3f();
    cv::Point3f predicted = cv::Point3f();
    manager.addTimeFunc(
        [&manager, &main_progress, &KF_Gain, &observed, &predicted]
        {
            KF_Gain = main_progress.getArmorDetector().predictor.getKFGain();
            observed = main_progress.getArmorDetector().predictor.getWorldCoordExtern();
            predicted = main_progress.getArmorDetector().predictor.getWorldCoordNextExtern();
        });
    auto my_switch_kf = std::make_shared<CustomControl>(string("KF Switch"));
    int my_switch_kf_result = 0;
    my_switch_kf->setShow(
        [&my_switch_kf, &my_switch_kf_result, &main_progress]
        {
            ImGui::Text("Kalman Filter Mode:");
            ImGui::RadioButton("X", &my_switch_kf_result, 0);
            ImGui::RadioButton("Y", &my_switch_kf_result, 1);
            ImGui::RadioButton("Z", &my_switch_kf_result, 2);
            if (ImGui::Button("Start Recording"))
            {
                main_progress.recording_enabled = true;
            }
            ImGui::SameLine();
            if (ImGui::Button("Stop Recording"))
            {
                main_progress.recording_enabled = false;
            }
            if (main_progress.recording_enabled)
            {
                ImGui::TextColored(ImVec4(1, 0, 0, 1), "Recording...");
            }
            else
            {
                ImGui::TextColored(ImVec4(0.5f, 0.5f, 0.5f, 1), "Not Recording");
            }
        });
    manager.AddControl(my_switch_kf);

    auto my_plot_kf_gain = std::make_shared<MyLinesPlot>(string("KF"), 100);
    my_plot_kf_gain->setUpdateDataFunc(
        [&my_plot_kf_gain, &main_progress, &my_switch_kf_result, &KF_Gain]()
        {
            static int last_result = 0;
            float y0 = 0;
            float y1 = 0;
            string str_tmp = string();
            if (last_result != my_switch_kf_result)
            {
                my_plot_kf_gain->Clear();
                last_result = my_switch_kf_result;
            }
            switch (my_switch_kf_result)
            {
            case 0:
                y0 = KF_Gain.at<float>(0, 0);
                y1 = KF_Gain.at<float>(1, 0);
                // str_tmp = string(" X");
                break;
            case 1:
                y0 = KF_Gain.at<float>(2, 1);
                y1 = KF_Gain.at<float>(3, 1);
                // str_tmp = string(" Y");
                break;
            case 2:
                y0 = KF_Gain.at<float>(4, 2);
                y1 = KF_Gain.at<float>(5, 2);
                // str_tmp = string(" Z");
                break;
            }

            my_plot_kf_gain->addOrUpdateSeries((string("KF_Gain") + str_tmp).c_str(), y0);
            my_plot_kf_gain->addOrUpdateSeries((string("KF_Gain_V") + str_tmp).c_str(), y1);
        });
    manager.AddControl(my_plot_kf_gain);

    auto my_world_x = std::make_shared<MyLinesPlot>(string("World_X"), 100);
    my_world_x->setUpdateDataFunc(
        [&my_world_x, &main_progress, &observed, &predicted]()
        {
            my_world_x->addOrUpdateSeries("Observed X", observed.x);
            my_world_x->addOrUpdateSeries("Predicted X", predicted.x);
        });
    manager.AddControl(my_world_x);

    auto my_world_y = std::make_shared<MyLinesPlot>(string("World_Y"), 100);
    my_world_y->setUpdateDataFunc(
        [&my_world_y, &main_progress, &observed, &predicted]()
        {
            my_world_y->addOrUpdateSeries("Observed Y", observed.y);
            my_world_y->addOrUpdateSeries("Predicted Y", predicted.y);
        });
    manager.AddControl(my_world_y);

    auto my_world_z = std::make_shared<MyLinesPlot>(string("World_Z"), 100);
    my_world_z->setUpdateDataFunc(
        [&my_world_z, &main_progress, &observed, &predicted]()
        {
            my_world_z->addOrUpdateSeries("Observed X", observed.z);
            my_world_z->addOrUpdateSeries("Predicted X", predicted.z);
        });
    manager.AddControl(my_world_z);

    auto dka = std::make_shared<DynamicKalmanAdjuster>(string("DKA"), main_progress.getArmorDetector().predictor);
    manager.AddControl(dka);

    auto world3d = std::make_shared<My3DPlot>("World_3D");
    world3d->setUpdateDataFunc(
        [&world3d, &main_progress, &observed, &predicted]()
        {
            BoxData red_ball{ImVec3(observed.x, observed.y, observed.z), ImVec3(0, 0, 0), ImVec3(0, 0, 0),
                             ImVec4(1, 0, 0, 1)};
            BoxData green_ball{ImVec3(predicted.x, predicted.y, predicted.z), ImVec3(0, 0, 0), ImVec3(0, 0, 0),
                               ImVec4(0, 1, 0, 1)};

            world3d->addOrUpdateBox("observed", red_ball);
            world3d->addOrUpdateBox("predicted", green_ball);
        });
    manager.AddControl(world3d);
    manager.Run();

#endif // debug_gui

    produce.join();
    consume.join();
    // fitting.join();
    // readport.join();
    // sendport.join();
    // savevideo.join();
}